#include "stdafx.h"
#include <xmmintrin.h>
#include <intrin.h>
#include <iostream>
using namespace std;

#ifndef QUATERNION_H
#define QUATERNION_H
class Quaternion
{

private:
	__m128 vec4;
	__m128 mulConstRow1;
	__m128 mulConstRow2;
	__m128 mulConstRow3;
	__m128 mulConstRow4;

public:
		inline Quaternion()
		{
			mulConstRow1 =_mm_set_ps(1.f, -1.f, -1.f, -1.f);
			mulConstRow2 =_mm_set_ps(1.0f, 1.f, 1.f, -1.f);
			mulConstRow3 =_mm_set_ps(1.0f, -1.f, 1.f, 1.f);
			mulConstRow4 =_mm_set_ps(1.0f, 1.f, -1.f, 1.f);
		}

		inline Quaternion(float x, float y, float z, float w)
		{
			vec4=_mm_set_ps(x, y, z, w);
			mulConstRow1 =_mm_set_ps(1.f, -1.f, -1.f, -1.f);
			mulConstRow2 =_mm_set_ps(1.f, 1.f, 1.f, -1.f);
			mulConstRow3 =_mm_set_ps(1.f, -1.f, 1.f, 1.f);
			mulConstRow4 =_mm_set_ps(1.f, 1.f, -1.f, 1.f);
		}

		inline __m128 GetVec4()
		{
			return vec4;
		}

		inline  Quaternion(const __m128& qword)
		{
			vec4=qword;
		}

		inline Quaternion& operator= (const Quaternion& copy)
		{
			vec4=copy.vec4;
			return *this;
		}

		inline Quaternion operator+(const Quaternion &b) const
		{
			return Quaternion(_mm_add_ps(vec4, b.vec4));
		}

		inline Quaternion operator-(const Quaternion &b) const
		{
			return Quaternion(_mm_sub_ps(vec4, b.vec4));
		}

		inline Quaternion operator*(const Quaternion &b) const
		{
			__m128 A2= _mm_shuffle_ps(vec4, vec4, _MM_SHUFFLE(2,3,1,0));

			__m128 B2= _mm_shuffle_ps(b.vec4, b.vec4, _MM_SHUFFLE(3,2,0,1));
			__m128 B3= _mm_shuffle_ps(b.vec4, b.vec4, _MM_SHUFFLE(1,0,3,2));
			__m128 B4= _mm_shuffle_ps(b.vec4, b.vec4, _MM_SHUFFLE(0,1,2,3));

			__m128 A1=_mm_mul_ps(vec4,mulConstRow1);
			       A2=_mm_mul_ps(A2,mulConstRow2);
			__m128 A3=_mm_mul_ps(vec4,mulConstRow3);
			__m128 A4=_mm_mul_ps(vec4,mulConstRow4);

			A1=_mm_mul_ps(A1,b.vec4);
			A2=_mm_mul_ps(A2,B2);
			A3=_mm_mul_ps(A3,B3);
			A4=_mm_mul_ps(A4,B4);

			A1 = _mm_hadd_ps(A1, A1);
			A1 = _mm_hadd_ps(A1, A1);

			A2 = _mm_hadd_ps(A2, A2);
			A2 = _mm_hadd_ps(A2, A2);

			A3 = _mm_hadd_ps(A3, A3);
			A3 = _mm_hadd_ps(A3, A3);

			A4 = _mm_hadd_ps(A4, A4);
			A4 = _mm_hadd_ps(A4, A4);

			float resultR1;
			_mm_store_ss(&resultR1, A1);

			float resultR2;
			_mm_store_ss(&resultR2, A2);

			float resultR3;
			_mm_store_ss(&resultR3, A3);

			float resultR4;
			_mm_store_ss(&resultR4, A4);
			

			return Quaternion(resultR1,resultR2,resultR3,resultR4);
		}


		inline Quaternion Conjugate()
		{
			return Quaternion(_mm_mul_ps(vec4,mulConstRow1));	
		}

		inline Quaternion Inverse()
		{
			return Quaternion(_mm_mul_ps(vec4, mulConstRow1));
		}

		inline float Abs()
		{
			__m128 temp=_mm_mul_ps(vec4,vec4);

			temp = _mm_hadd_ps(temp, temp);
			temp = _mm_hadd_ps(temp, temp);

			float result;
			_mm_store_ss(&result, temp);
			return result;
			
			return sqrt(result);
		}

		inline float* GetArray()
		{
			float *temp = (float*) _aligned_malloc(4 * sizeof(float), 16);
			_mm_storer_ps(temp,vec4);
			return temp;
		}

		void PrintQuaternion()
		{
			float* temp=GetArray();
			std::cout<<temp[0]<<" + "<<temp[1]<<"i + "<<temp[2]<<"j + "<<temp[3]<<"k"<<std::endl;
		}


		inline float HorizontalSum()
		{
			__m128 temp;
			temp= _mm_hadd_ps(vec4, vec4);
			temp =_mm_hadd_ps(temp, temp);

			float result;
			_mm_store_ss(&result, temp);
			return result;
		}
		
		

		inline Quaternion Norm()
		{
			float n= Abs();
			__m128 temp=_mm_set_ps1(n);
			return Quaternion(_mm_div_ps(vec4,temp));
		}

		
};
#endif